import time
from multi_robomaster import multi_robot
import threading

from ep_staticatch_rule import EP
import pos_nokov as pos
import center_control as cc
from ep_def import EP_def

if __name__=='__main__':
    event = threading.Event()
    pt = threading.Thread(target=pos.run,args=(event, ))
    pt.start()
    ct = threading.Thread(target=cc.run,args=(event, ))
    ct.start()
    
    
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', 
                      '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()
    
    number = multi_robots.number_id_by_sn([1, robots_sn_list[0]],
                                          [3, robots_sn_list[2]],
                                          [5, robots_sn_list[4]],
                                          [6, robots_sn_list[5]])
    print("The number of robot is: {0}".format(number))
    # g_all = multi_robots.build_group([2, 3])
    g1 = multi_robots.build_group([1])
    # g2 = multi_robots.build_group([2])
    g3 = multi_robots.build_group([3])
    g5 = multi_robots.build_group([5])
    g6 = multi_robots.build_group([6])
    
    # tm1 = threading.Thread(target=tm1.run, args=(multi_robots,g2))
    # tm1.start()
    
    ep1=EP(multi_robots, g1, id=1, role='chase')
    ep3=EP(multi_robots, g3, id=3)
    ep5=EP_def(multi_robots, g5, id=5)
    ep6=EP_def(multi_robots, g6, id=6)
    t1 = threading.Thread(target=ep1.run,args=(event, ))
    t1.start()
    t3 = threading.Thread(target=ep3.run,args=(event,))
    t3.start()
    
    # t5 = threading.Thread(target=ep5.run,args=(event,))
    # t5.start()
    # t6 = threading.Thread(target=ep6.run,args=(event,))
    # t6.start()

